using System;

using OpenTK;

namespace ColladaDotNet.OpenTKMapping
{
	public static class MatrixExt
	{
		public static Quaterniond EvalQuaternion(ref	Matrix4d m)
		{
			double qw, qx, qy, qz;

			double tr = m.M11 + m.M22 + m.M33;
			if (tr > 0)
			{
				double S = (double)Math.Sqrt(tr + 1.0) * 2; // S=4*qw 
				qw = 0.25f * S;
				qx = (m.M23 - m.M32) / S;
				qy = (m.M31 - m.M13) / S;
				qz = (m.M12 - m.M21) / S;
			}
			else if ((m.M11 > m.M22) & (m.M11 > m.M33))
			{
				double S = (float)Math.Sqrt(1.0f + m.M11 - m.M22 - m.M33) * 2; // S=4*qx 
				qw = (m.M23 - m.M32) / S;
				qx = 0.25f * S;
				qy = (m.M21 + m.M12) / S;
				qz = (m.M31 + m.M13) / S;
			}
			else if (m.M22 > m.M33)
			{
				double S = (float)Math.Sqrt(1.0f + m.M22 - m.M11 - m.M33) * 2; // S=4*qy
				qw = (m.M31 - m.M13) / S;
				qx = (m.M21 + m.M12) / S;
				qy = 0.25f * S;
				qz = (m.M32 + m.M23) / S;
			}
			else
			{
				double S = (float)Math.Sqrt(1.0f + m.M33 - m.M11 - m.M22) * 2; // S=4*qz
				qw = (m.M12 - m.M21) / S;
				qx = (m.M31 + m.M13) / S;
				qy = (m.M32 + m.M23) / S;
				qz = 0.25f * S;
			}

			return new Quaterniond(qx, qy, qz, qw);
		}
		public static Quaternion EvalQuaternion(ref	Matrix4 m)
		{
			float qw, qx, qy, qz;

			float tr = m.M11 + m.M22 + m.M33;
			if (tr > 0)
			{
				float S = (float)Math.Sqrt(tr + 1.0) * 2; // S=4*qw 
				qw = 0.25f * S;
				qx = (m.M23 - m.M32) / S;
				qy = (m.M31 - m.M13) / S;
				qz = (m.M12 - m.M21) / S;
			}
			else if ((m.M11 > m.M22) & (m.M11 > m.M33))
			{
				float S = (float)Math.Sqrt(1.0f + m.M11 - m.M22 - m.M33) * 2; // S=4*qx 
				qw = (m.M23 - m.M32) / S;
				qx = 0.25f * S;
				qy = (m.M21 + m.M12) / S;
				qz = (m.M31 + m.M13) / S;
			}
			else if (m.M22 > m.M33)
			{
				float S = (float)Math.Sqrt(1.0f + m.M22 - m.M11 - m.M33) * 2; // S=4*qy
				qw = (m.M31 - m.M13) / S;
				qx = (m.M21 + m.M12) / S;
				qy = 0.25f * S;
				qz = (m.M32 + m.M23) / S;
			}
			else
			{
				float S = (float)Math.Sqrt(1.0f + m.M33 - m.M11 - m.M22) * 2; // S=4*qz
				qw = (m.M12 - m.M21) / S;
				qx = (m.M31 + m.M13) / S;
				qy = (m.M32 + m.M23) / S;
				qz = 0.25f * S;
			}
			
			return new Quaternion(qx,qy,qz,qw);
		}
	}
}